import rclpy
from rclpy.node import Node
from gazebo_msgs.srv import SetEntityState
from geometry_msgs.msg import Pose, Point, Quaternion
import math

class MovingObstacle(Node):
    def __init__(self):
        super().__init__('moving_obstacle')
        self.client = self.create_client(SetEntityState, '/world/eed.world/set_entity_state')  # 替换为你的世界名称
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('等待服务可用...')
        self.timer = self.create_timer(0.1, self.update_position)
        self.start_time = self.get_clock().now().seconds_nanoseconds()[0]
        self.point_a = [0.0, 0.0, 0.5]  # 起始点A
        self.point_b = [2.0, 0.0, 0.5]  # 结束点B
        self.duration = 5.0  # 单程时间（秒）

    def update_position(self):
        current_time = self.get_clock().now().seconds_nanoseconds()[0] - self.start_time
        t = current_time % (2 * self.duration)
        if t > self.duration:
            t = 2 * self.duration - t
        ratio = math.sin((t / self.duration) * math.pi / 2)  # 平滑移动
        x = self.point_a[0] + (self.point_b[0] - self.point_a[0]) * ratio
        y = self.point_a[1] + (self.point_b[1] - self.point_a[1]) * ratio
        z = self.point_a[2]
        pose = Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(w=1.0))
        req = SetEntityState.Request()
        req.state.name = 'moving_cylinder'
        req.state.pose = pose
        req.state.reference_frame = 'world'
        future = self.client.call_async(req)
        future.add_done_callback(self.service_response_callback)

    def service_response_callback(self, future):
        try:
            response = future.result()
            if not response.success:
                self.get_logger().error('更新位置失败')
        except Exception as e:
            self.get_logger().error(f'服务调用失败: {e}')

def main(args=None):
    rclpy.init(args=args)
    node = MovingObstacle()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()